
/*
* core zero, proc message.
*/

#include <stdint.h>
#include <stdio.h>

#include "rp2040.h"
#include "hardware/clocks.h"
#include "hardware/uart.h"

#include "pico.h"
#include "pico/multicore.h"

#include "msgpool.h"
#include "umsg.h"


static inline uint32_t  read_reg( uintptr_t addr )
{
    return (*((volatile uint32_t *) (addr)));
}


int  core_zero_rreg( umsg_t * pmsg )
{
	uint32_t ofs;
	uint32_t cnt;
	uint8_t * ptr;
	int i;

	/**/
	ptr = (uint8_t *)&pmsg->data[0];
	ofs = *(uint32_t *)ptr;
	ptr += 4;
	cnt = *(uint32_t *)ptr;
	ptr += 4;

	/**/
	ptr = (uint8_t *)&pmsg->data[0];
	for ( i=0; i<8; i++ ) {
		*(uint32_t *)ptr = read_reg( ofs + (i * 4) );
		ptr += 4;
	}

	/**/
	pmsg->mlen = 32;
	umsg_send( pmsg );
	return 0;
}


int core_zero_clock( umsg_t * pmsg )
{
	int idx;
	int i;
	uint8_t * ptr;
	uint32_t  hzhz;

	/**/
	idx = pmsg->data[0];
	hzhz = clock_get_hz( idx );

	/**/
	ptr = (uint8_t *)&pmsg->data[0];
	*(uint32_t *)ptr = hzhz;

	/**/
	pmsg->mlen = 4;
	umsg_send( pmsg );
	return 0;
}


int core_zero_u1send( umsg_t * pmsg )
{
	int i;

	for ( i=0; i<pmsg->mlen; i++ ) {
		uart_putc_raw( uart0, pmsg->data[i] );
	}

	/**/
	pmsg->mlen = 0;
	umsg_send( pmsg );
	return 0;
}



void  core_zero_proc_msg( void )
{
	umsg_t * pmsg;

	while ( true )  {

		pmsg = umsg_recv();
		if ( pmsg == NULL ) {
			break;
		}

		if ( pmsg->type < 0x80 ) {
			/* forward to core-1 */
			multicore_fifo_push_blocking( (uint32_t)(uintptr_t)pmsg );
			continue;
		}

		// printf( "recv, %p\n", pmsg );

		switch ( pmsg->type) {

		case 0x80:
			core_zero_rreg( pmsg );
			break;

		case 0x81:
			core_zero_clock( pmsg );
			break;

		case 0x82:
			core_zero_u1send( pmsg );
			break;

		default:
			break;

		}

	}

	return;
}


